#ifndef _MODULEBASE_H
#define _MODULEBASE_H

#include <unordered_map>
#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <boost/lexical_cast.hpp>
#include "boost/type_index.hpp"

#include "ros/ros.h"
#include <ros/package.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include "json/json.h"
#include "qomolo_msgs/WarningCode.h"
#include "qomolo_msgs/ErrorCode.h"
#include "qomolo_msgs/MonitorSrv.h"
#include "qomolo_msgs/MonitorInfo.h"
#include "module_base/util.h"

#define CREATE_SUBSCRIPION(topic_name,callback,queue_size)                     \
{                                                                              \
	subscribers_[topic_name]=nh_.subscribe(topic_name,                         \
		queue_size,callback,this);                                                     \
} 

namespace module_base
{
class ModuleBase : public nodelet::Nodelet
{
protected:
	ros::NodeHandle nh_;
	ros::NodeHandle pnh_;
	std::unordered_map<std::string, std::string> params_;
	std::unordered_map<std::string, ros::Publisher> publishers_;
	std::unordered_map<std::string, ros::Subscriber> subscribers_;
	std::string module_name_;
	VehicleParam vehicle_param_;

	template<typename T>
	void CREATE_PUBLISHER(std::string ID,std::string topic_name,
		int queue_size, bool keep=false)
	{
		publishers_[ID]=nh_.advertise<T>(topic_name,queue_size,keep);
	}

	void CREATE_MONITOR_PUBLISHER(std::string topic_name, 
		int queue_size, bool keep=false)
	{
		publishers_["monitor"] = nh_.advertise<qomolo_msgs::MonitorInfo>(topic_name,queue_size,keep);
	}

public:
	ModuleBase(){};
	~ModuleBase(){};

	// virtual void onInit(){};

	// virtual void loop();

	void init_nodelet()
	{
		nh_ = getNodeHandle();
    	pnh_ = getPrivateNodeHandle();
    	std::string node_name=getName();

    	ROS_INFO("node_name: %s",node_name.c_str());
    	std::vector<std::string> param_names;
    	if(pnh_.getParamNames(param_names))
    	{
		    for(std::string name : param_names)
		    {
		    	std::string param_name;
		    	bool valid = get_param_name(node_name,name,param_name);
		    	if (valid)
		    	{
		    		std::string param_value;
			    	pnh_.getParam(name, param_value);
			        ROS_INFO("settings: %s,%s",param_name.c_str(),param_value.c_str());
			        params_[param_name]=param_value;
		    	}
		    }
		}


		CREATE_PUBLISHER<qomolo_msgs::WarningCode>("warning","warning_code",1,false);
		CREATE_PUBLISHER<qomolo_msgs::ErrorCode>("error","error_code",1,false);
		// std::string topic_str="topic";
		// for (auto it=params_.begin();it!=params_.end();it++)
		// {
		// 	std::string::size_type idx=it->first.find(topic_str);
		// 	if(idx == std::string::npos )
		// 	{
		// 		continue;
		// 	}
		// 	printf ("topic name: %s\n",it->first.c_str());
		// }
	}

	bool get_param_name(std::string &node_name,std::string &name,
		std::string &param_name)
	{
		std::stringstream ss(name);
		// std::string elem;
		bool valid=false;
		while (getline(ss, param_name, '/')) 
		{
			if ("/"+param_name==node_name)
			{
				valid=true;
			}
		}
		// ROS_INFO("param name: %d,%s",valid,elem.c_str());
		return valid;
	}

	void warning_publish(int code, const std::string& info)
	{
		qomolo_msgs::WarningCode warning_msg;
		warning_msg.header.stamp = ros::Time::now();
		warning_msg.warning_code = code;
		warning_msg.warning_info = info;
		publishers_["warning"].publish(warning_msg);
	}

	void error_publish(int code, const std::string& info)
	{
		qomolo_msgs::ErrorCode error_msg;
		error_msg.header.stamp = ros::Time::now();
		error_msg.error_code = code;
		error_msg.error_info = info;
		publishers_["error"].publish(error_msg);
	}

	std::string wait_for_monitor(const std::string& module_name)
	{
		module_name_ = module_name;

		ros::service::waitForService("/monitor_service");
		ros::ServiceClient monitor_client = nh_.serviceClient<qomolo_msgs::MonitorSrv>("/monitor_service", false);
		monitor_client.waitForExistence();
		qomolo_msgs::MonitorSrv srv_req;
		srv_req.request.module_name = module_name;
		std::string topic_id = "";
		while(true)
		{
			if(monitor_client.call(srv_req))
			{
				topic_id = srv_req.response.topic_id;
				break;
			}
			else
			{
				ros::Duration(0.5).sleep();
			}
		}
		return topic_id;
	}

	void monitor_info_publish(int status)
	{
		qomolo_msgs::MonitorInfo monitor_msg;
		monitor_msg.header.stamp = ros::Time::now();
		monitor_msg.module_name = module_name_;
		monitor_msg.status = status;

		publishers_["monitor"].publish(monitor_msg);
	}


	bool parse_vehicle_param(std::string param_file)
	{
		std::ifstream in(param_file, std::ios::binary);

		if (!in.is_open())
		{
			error_publish(4,"vehicle param file open fail");
			return false;
		}

		Json::Reader reader;
		Json::Value vehicle_param_json;
		// 解析json内容
		if (!reader.parse(in, vehicle_param_json))
		{
			error_publish(4,"vehicle param  file open fail");
			return false;
		}

		std::cout<<vehicle_param_json<<std::endl;
		in.close();

		vehicle_param_.back_edge_to_center=vehicle_param_json["back_edge_to_center"].asFloat();
		vehicle_param_.back_edge_to_center_t=vehicle_param_json["back_edge_to_center_t"].asFloat();
		vehicle_param_.hitch_t=vehicle_param_json["hitch_t"].asFloat();
		vehicle_param_.hitch_v=vehicle_param_json["hitch_v"].asFloat();
		vehicle_param_.kinpain=vehicle_param_json["kinpain"].asFloat();
		vehicle_param_.max_acceleration=vehicle_param_json["max_acceleration"].asFloat();
		vehicle_param_.max_angle_tractor_trailer=vehicle_param_json["max_angle_tractor_trailer"].asFloat();
		vehicle_param_.max_deceleration=vehicle_param_json["max_deceleration"].asFloat();
		vehicle_param_.max_steer_angle=vehicle_param_json["max_steer_angle"].asFloat();
		vehicle_param_.max_steer_angle_rate=vehicle_param_json["max_steer_angle_rate"].asFloat();
		vehicle_param_.min_turn_radius=vehicle_param_json["min_turn_radius"].asFloat();
		vehicle_param_.t_length=vehicle_param_json["t_length"].asFloat();
		vehicle_param_.t_width=vehicle_param_json["t_width"].asFloat();
		vehicle_param_.v_length=vehicle_param_json["v_length"].asFloat();
		vehicle_param_.v_width=vehicle_param_json["v_width"].asFloat();
		vehicle_param_.wheel_base=vehicle_param_json["wheel_base"].asFloat();
		if (vehicle_param_json.isMember("wheel_base2"))
		{
			vehicle_param_.wheel_base2=vehicle_param_json["wheel_base2"].asFloat();
		}
		vehicle_param_.wheel_radius=vehicle_param_json["wheel_radius"].asFloat();
		vehicle_param_.wheel_width=vehicle_param_json["wheel_width"].asFloat();
		return true;
	}
};//class

}//namespace

#endif